import numpy as np


def degreeToRadians(x):
    return x / 360 * 2 * np.pi


def radiansToDegree(x):
    return 360 * (x / (2 * np.pi))


def getRotationMatrix(alpha, beta, gamma):
    """
    Args:
        alpha: degree 绕x轴旋转的角度
        beta: degree 绕y轴旋转的角度
        gamma: degree 绕z轴旋转的角度

    Returns: RotationMatrix
    """
    Mx = np.mat([[1, 0, 0],
                 [0, np.cos(alpha), np.sin(alpha)],
                 [0, -np.sin(alpha), np.cos(alpha)]])
    My = np.mat([[np.cos(beta), 0, -np.sin(beta)],
                 [0, 1, 0],
                 [np.sin(beta), 0, np.cos(beta)]])
    Mz = np.mat([[np.cos(gamma), np.sin(gamma), 0],
                 [-np.sin(gamma), np.cos(gamma), 0],
                 [0, 0, 1]])
    return Mx * My * Mz


# 相机光心在世界坐标系中的位置
Camera_position_x = 15
Camera_position_y = -10
Camera_position_z = -10
# 相机在世界坐标系中旋转轴
Camera_rotation_x = 0  # radiansToDegree(np.arctan(1/np.sqrt(6)))
Camera_rotation_y = 30  # -radiansToDegree(np.arctan(np.sqrt(3) / 2))  # -45
Camera_rotation_z = -45

f = 35  # 焦距mm8
Cmos_y = 23.76  # CMOS宽度mm
Cmos_z = 13.365  # CMOS长度mm
Pix_y = 1920  # 像素
Pix_z = 1080  # 像素
u0 = 0  #
v0 = 0  #

# 世界坐标系中的坐标点
Xw = -1.61
Yw = -0.63
Zw = 0.83
# 畸变参数
k1, k2, p1, p2, k3 = -4.96438957e-03, 8.29483211e-02, 6.22011460e-05, 2.13799300e-04, -3.02606096e-01
# k1, k2, p1, p2, k3 = 1.82689808e-02, -4.80714810e-01, -6.69713867e-04, 6.30564094e-07, 3.23660261e+00

W = np.mat([[Xw], [Yw], [Zw], [1]])

R2 = getRotationMatrix(degreeToRadians(Camera_rotation_x),
                       degreeToRadians(Camera_rotation_y),
                       degreeToRadians(Camera_rotation_z))



RT = np.hstack((R2, -R2 * np.mat([[Camera_position_x], [Camera_position_y], [Camera_position_z]])))  # 外参
M2 = np.vstack((RT, [0, 0, 0, 1]))

print(M2)
print(M2*W)

M11 = np.mat([[1, 0, 0, 0],
              [0, -f, 0, 0],
              [0, 0, -f, 0]])

# 畸变矫正
Re1 = M2 * W
y1 = Re1[1, 0] / Re1[0, 0]
z1 = Re1[2, 0] / Re1[0, 0]
r = np.sqrt(y1 ** 2 + z1 ** 2)
y2 = y1 * (1 + k1 * r ** 2 + k2 * r ** 4 + k3 * r ** 6) + 2 * p1 * y1 * z1 + p2 * (r ** 2 + 2 * y1 ** 2)
z2 = z1 * (1 + k1 * r ** 2 + k2 * r ** 4 + k3 * r ** 6) + 2 * p2 * y1 * z1 + p1 * (r ** 2 + 2 * z1 ** 2)
fy = -f / (Cmos_y / Pix_y)
fz = -f / (Cmos_z / Pix_z)

print(y1, z1)
print(y2, z2)
ud = fy * y2
vd = fz * z2
print(ud, vd)

M12 = np.mat([[1, 0, 0],
              [u0, 1 / (Cmos_y / Pix_y), 0],
              [v0, 0, 1 / (Cmos_z / Pix_z)]])

M1 = M12 * M11
# M1 = np.mat([[1, 0, 0, 0],
#              [u0, -f / (Cmos_y / Pix_y), 0, 0],
#              [v0, 0, -f / (Cmos_z / Pix_z), 0]])

Re = M1 * M2 * W
print(Re)

# print("y:", Re[1, 0] / Re[0, 0])
# print("z:", Re[2, 0] / Re[0, 0])
#
# y1 = Re[1, 0] / Re[0, 0]
# z1 = Re[2, 0] / Re[0, 0]
